#AUTO
A_BUSY=1;B_BUSY=1;C_BUSY=1;D_BUSY=1
SHTCLR=100000
SHTSET=50000
SEARCH=100000
A_BUSY=0;B_BUSY=0;C_BUSY=0;D_BUSY=0
A_OFFSET=0;B_OFFSET=0;C_OFFSET=0;D_OFFSET=0
EN
#A_SPEED
IF A_SPD=1;ACA=20000;DCA=20000;SPA=5000;ENDIF
IF A_SPD=2;ACA=100000;DCA=100000;SPA=100000;ENDIF
IF A_SPD=3;ACA=200000;DCA=200000;SPA=100000;ENDIF
IF A_SPD=4;ACA=200000;DCA=200000;SPA=100000;ENDIF
EN
#B_SPEED
IF B_SPD=1;ACB=20000;DCB=20000;SPB=5000;ENDIF
IF B_SPD=2;ACB=100000;DCB=100000;SPB=100000;ENDIF
IF B_SPD=3;ACB=200000;DCB=200000;SPB=100000;ENDIF
IF B_SPD=4;ACB=200000;DCB=200000;SPB=100000;ENDIF
EN
#C_SPEED
IF C_SPD=1;ACC=20000;DCC=20000;SPC=5000;ENDIF
IF C_SPD=2;ACC=100000;DCC=100000;SPC=100000;ENDIF
IF C_SPD=3;ACC=200000;DCC=200000;SPC=100000;ENDIF
IF C_SPD=4;ACC=200000;DCC=200000;SPC=100000;ENDIF
EN
#D_SPEED
IF D_SPD=1;ACD=20000;DCD=20000;SPD=5000;ENDIF
IF D_SPD=2;ACD=100000;DCD=100000;SPD=100000;ENDIF
IF D_SPD=3;ACD=200000;DCD=200000;SPD=100000;ENDIF
IF D_SPD=4;ACD=200000;DCD=200000;SPD=100000;ENDIF
EN
#X_ALL;XQ #A_ALL,1;XQ #B_ALL,2;XQ #C_ALL,3;XQ #D_ALL,4;EN
#X_ZERO;XQ #A_ZERO,1;XQ #B_ZERO,2;XQ #C_ZERO,3;XQ #D_ZERO,4;EN
#X_SW1;XQ #A_SW1,1;XQ #B_SW1,2;XQ #C_SW1,3;XQ #D_SW1,4;EN
#X_SW2;XQ #A_SW2,1;XQ #B_SW2,2;XQ #C_SW2,3;XQ #D_SW2,4;EN
#X_SW3;XQ #A_SW3,1;XQ #B_SW3,2;XQ #C_SW3,3;XQ #D_SW3,4;EN
#A_ALL
A_BUSY=1;A_SPD=2;JS #A_SPEED;IF(_TSA&$04=$00);PRA=500000;BGA;MCA;ENDIF
PRA=-20000000;BGA;MG "A: DRIVING NEGATIVE TO LIMIT SWITCH"{EA}
#A_LP01;IF(_TSA&$02=$02)|(_TI0&$01=$01);ACA=500000;SPA=125000;ENDIF
IF(_TI0&$02=$02);ACA=200000;SPA=50000;ENDIF;JP #A_LP01,(_TSA&$04=$04);MCA
MG "A: NEGATIVE LIMIT SWITCH TRIPPED"{EA}
MG "A: DRIVING FORWARD SLOWLY WATCHING FOR LIMIT SWITCH TO CLEAR"{EA}
A_SPD=1;JS #A_SPEED;PRA=500000;BGA
#A_LP02;JP #A_LP02,(_TSA&$04=$00)
STA;MCA;DPA=0;MG "A: LIMIT SWITCH CLEAR - ZERO DEFINED"{EA}
A_SPD=2;JS #A_SPEED;PRA=-1000000;BGA;
MG "A: DRIVING BACK TO TRIP & STORING LOCATION OF LIMIT SWITCH"{EA}
#A_LP03;JP #A_LP03,(_TSA&$04=$04);MCA;A_LL_SET=_PAA
MG "A: LOW LIMIT SWITCH FOUND AT "{EA},A_LL_SET{EA} {F6.0}
A_SPD=2;JS #A_SPEED;PRA=2000000;BGA
MG "A: DRIVING FORWARD WATCHING FOR SW1 TO TRIP"{EA}
#A_LP04;JP #A_LP04,(_TI0&$02=$00);STA;MCA;A_S1_SET=_PAA
MG "A: SW1 TRIPPED AT "{EA},A_S1_SET{EA}{F6.0}
A_SPD=1;JS #A_SPEED;PRA=-500000
MG "A: DRIVING NEGATIVE SLOWLY WATCHING FOR SW1 TO CLEAR"{EA};BGA
#A_LP05;JP #A_LP05,(_TI0&$02=$02);STA;MCA;A_S1_CLR=_PAA
MG "A: SW1 CLEARED AT "{EA},A_S1_CLR{EA}{F6.0}
MG "A: DRIVING FORWARD WATCHING FOR SW2 TO TRIP"{EA}
PRA=10000000;A_SPD=3;JS #A_SPEED;BGA
#A_LP06;JP #A_LP06,(_TI0&$01=$00);STA;MCA;A_S2_SET=_PAA
MG "A: SW2 TRIPPED AT "{EA},A_S2_SET{EA}{F7.0}
A_SPD=1;JS #A_SPEED;PRA=-500000
MG "A: DRIVING NEGATIVE SLOWLY WATCHING FOR SW2 TO CLEAR"{EA};BGA
#A_LP07;JP #A_LP07,(_TI0&$01=$01);STA;MCA;A_S2_CLR=_PAA
MG "A: SW2 CLEARED AT "{EA},A_S2_CLR{EA}{F7.0}
MG "A: DRIVING FORWARD WATCHING FOR SW3 TO TRIP"{EA}
PRA=15000000;A_SPD=3;JS #A_SPEED;BGA
#A_LP08;JP #A_LP08,(_TSA&$02=$00);STA;MCA;A_S3_SET=_PAA
MG "A: SW3 TRIPPED AT "{EA},A_S3_SET{EA}{F8.0}
A_SPD=2;JS #A_SPEED;PRA=-500000
MG "A: DRIVING NEGATIVE WATCHING FOR SW3 TO CLEAR"{EA};BGA
#A_LP09;JP #A_LP09,(_TSA&$02=$02);STA;MCA;A_S3_CLR=_PAA
MG "A: SW3 CLEARED AT "{EA},A_S3_CLR{EA}{F8.0}
MG "A: DRIVING FORWARD WATCHING FOR HIGH LIMIT TO TRIP"{EA}
PRA=10000000;A_SPD=2;JS #A_SPEED;BGA
#A_LP10;JP #A_LP10,(_TSA&$08=$08);MCA;A_HL_SET=_PAA
MG "A: HIGH LIMIT SWITCH FOUND AT "{EA},A_HL_SET{EA}{F8.0}
A_SPD=2;JS #A_SPEED;PRA=-500000
MG "A: DRIVING NEGATIVE SLOWLY WATCHING FOR HIGH LIMIT TO CLEAR"{EA};BGA
#A_LP11;JP #A_LP11,(_TSA&$08=$00);MCA;A_HL_CLR=_PAA
MG "A: HIGH LIMIT SWITCH CLEARED AT "{EA},A_HL_CLR{EA}{F8.0}
MG ""{EA}
MG "A_LL_SET/A_LL_CLR"{EA},A_LL_SET{EA}{F5.0},"/0"{EA}
MG "A_S1_SET/A_S1_CLR"{EA},A_S1_SET{EA}{F6.0},"/"{EA},A_S1_CLR{EA}{F6.0}
MG "A_S2_SET/A_S2_CLR"{EA},A_S2_SET{EA}{F7.0},"/"{EA},A_S2_CLR{EA}{F7.0}
MG "A_S3_SET/A_S3_CLR"{EA},A_S3_SET{EA}{F8.0},"/"{EA},A_S3_CLR{EA}{F8.0}
MG "A_HL_SET/A_HL_CLR"{EA},A_HL_SET{EA}{F8.0},"/"{EA},A_HL_CLR{EA}{F8.0}
MG "A: DONE!"{EA};A_BUSY=0;EN
#A_ZERO;A_BUSY=1;A_POS=0;A_SPD=4;JS #A_SPEED
IF(_TSA&$04=$00);MG "A: LOW LIMIT ALREADY ACTUATED - DRIVING POSITIVE"{EA}
PRA=100000;BGA;MCA;ENDIF
PAA=A_LL_SET;MG "A: DRIVING TO A_LL_SET"{EA};BGA;MCA
IF(_TSA&$04=$04)
MG "A: LOW LIMIT SWITCH NOT FOUND WHERE EXPECTED - SEARCHING"{EA}
A_SPD=2;JS #A_SPEED;PRA=-20000000;BGA
#A_LP12;IF(_TSA&$02=$02)|(_TI0&$01=$01);ACA=500000;SPA=125000;ENDIF
IF(_TI0&$02=$02);ACA=200000;SPA=50000;ENDIF
JP #A_LP12,(_TSA&$04=$04)&(_BGA=1);MCA;ENDIF
IF(_TSA&$04=$00)
MG "A: LOWER LIMIT TRIPPED, WATCHING FOR RESET"{EA}
A_SPD=1;JS #A_SPEED;PRA=SEARCH;BGA
#A_LP13
JP #A_LP13,(_TSA&$04=$00)&(_BGA=1);STA;MCA
ELSE
MG "COULD NOT FIND NEGATIVE LIMIT SWITCH"{EA}
A_POS=0;A_ERR=1;ENDIF
IF(_TSA&$04=$04);DPA=0;MG "A: ZERO DEFINED AT LOWER LIMIT"{EA};A_POS=0;A_ERR=0
ELSE;MG "A: LOWER LIMIT CLEAR NOT FOUND IN SPECIFIED SEARCH DISTANCE"{EA}
ENDIF;MG "A: DONE!"{EA};A_BUSY=0;EN
#A_SW1;A_BUSY=1;A_POS=0
A_SPD=4;JS #A_SPEED;PAA=A_S1_CLR-SHTCLR;MG "A: DRIVING TO A_S1_CLR-SHTCLR"{EA}
BGA;MCA
A_SPD=2;JS #A_SPEED;PAA=A_S1_SET-SHTSET;MG "A: DRIVING TO A_S1_SET-SHTSET"{EA}
BGA;MCA
A_SPD=1;JS #A_SPEED;PRA=SEARCH;MG "A: WATCHING FOR SW1 TO SET"{EA};BGA
#A_LP14
JP #A_LP14,(_TI0&$02=$00)&(_BGA=1);STA;MCA
IF(_TI0&$02=$02)&(_PAA>(A_S1_SET-SHTSET))
A_S1_CUR=_PAA;A_POS=1;A_ERR=0;MG "A: SW1 SET @"{EA},A_S1_CUR{EA} {F6.0}
MG "A: A_S1_SET="{EA},A_S1_SET{EA} {F6.0};ENDIF
IF(_TI0&$02=$02)&(_PAA=(A_S1_SET-SHTSET))
MG "A: SW1 FOUND BEFORE MOTOR MOVED"{EA};ENDIF
IF(_TI0&$02=$00)&(_PAA=(A_S1_SET-SHTSET+SEARCH))
A_POS=-1;A_ERR=1;MG "A: SW1 NOT FOUND WITHIN SPECIFIED DISTANCE"{EA};ENDIF
IF(_TSA&04=$00)
A_POS=-1;A_ERR=1;MG "A: SW1 - NEGATIVE LIMIT TRIPPED"{EA};ENDIF
IF(_TSA&08=$00)
A_POS=-1;A_ERR=1;MG "A: SW1 - POSITIVE LIMIT TRIPPED"{EA};ENDIF
IF(A_ERR=0)
SPA=50000
PRA=A_OFFSET;BGA;MCA
ENDIF
MG "A: DONE!"{EA};A_BUSY=0;EN
#A_SW2;A_BUSY=1;A_SPD=4;JS #A_SPEED;PAA=A_S2_CLR-SHTCLR
MG "A: DRIVING TO A_S2_CLR - SHTCLR"{EA};BGA;MCA
A_SPD=2;JS #A_SPEED;PAA=A_S2_SET-SHTSET
MG "A: DRIVING TO A_S2_SET - SHTSET"{EA};BGA;MCA
A_SPD=1;JS #A_SPEED;PRA=SEARCH;MG "A: WATCHING FOR SW2 TO SET"{EA};BGA
#A_LP15
JP #A_LP15,(_TI0&$01=$00)&(_BGA=1);STA;MCA
IF(_TI0&$01=$01)&(_PAA>(A_S2_SET-SHTSET))
A_S2_CUR=_PAA;MG "A: SW2 SET @"{EA},A_S2_CUR{EA} {F7.0}
MG "A: A_S2_SET="{EA},A_S2_SET{EA} {F7.0};A_POS=2;A_ERR=0;ENDIF
IF(_TI0&$01=$01)&(_PAA=(A_S2_SET-SHTSET))
MG "A: SW2 FOUND BEFORE MOTOR MOVED"{EA};ENDIF
IF(_TI0&$01=$00)&(_PAA=(A_S2_SET-SHTSET+SEARCH))
A_POS=-1;A_ERR=1;MG "A: SW2 NOT FOUND WITHIN SPECIFIED DISTANCE"{EA};ENDIF
IF(_TSA&04=$00)
A_POS=-1;A_ERR=1;MG "A: SW2 - NEGATIVE LIMIT TRIPPED"{EA};ENDIF
IF(_TSA&08=$00)
A_POS=-1;A_ERR=1;MG "A: SW2 - POSITIVE LIMIT TRIPPED"{EA};ENDIF
IF(A_ERR=0)
SPA=50000
PRA=A_OFFSET;BGA;MCA
ENDIF
MG "A: DONE!"{EA};A_BUSY=0;EN
#A_SW3;A_BUSY=1;A_SPD=4;JS #A_SPEED;PAA=A_S3_CLR-SHTCLR
MG "A: DRIVING TO A_S3_CLR - SHTCLR"{EA};BGA;MCA
A_SPD=2;JS #A_SPEED;PAA=A_S3_SET-SHTSET
MG "A: DRIVING TO A_S3_SET - SHTSET"{EA}
BGA;MCA
A_SPD=1;JS #A_SPEED;PAA=A_S3_SET-SHTSET+SEARCH
MG "A: WATCHING FOR SW3 TO SET"{EA};BGA
#A_LP16
JP #A_LP16,(_TSA&$02=$00)&(_BGA=1);STA;MCA
IF(_TSA&$02=$02)&(_PAA>(A_S3_SET-SHTSET))
A_S3_CUR=_PAA;MG "A: SW3 SET @"{EA},A_S3_CUR{EA} {F8.0}
MG "A: A_S3_SET="{EA},A_S3_SET{EA} {F8.0};A_POS=3;A_ERR=0;ENDIF
IF(_TSA&$02=$02)&(_PAA=(A_S3_SET-SHTSET))
MG "A: SW3 FOUND BEFORE MOTOR MOVED"{EA};ENDIF
IF(_TSA&$02=$00)&(_PAA=(A_S3_SET-SHTSET+SEARCH))
A_POS=-1;A_ERR=1;MG "A: SW3 NOT FOUND WITHIN SPECIFIED DISTANCE"{EA};ENDIF
IF(_TSA&04=$00)
A_POS=-1;A_ERR=1;MG "A: SW3 - NEGATIVE LIMIT TRIPPED"{EA};ENDIF
IF(_TSA&08=$00)
A_POS=-1;A_ERR=1;MG "A: SW3 - POSITIVE LIMIT TRIPPED"{EA};ENDIF
IF(A_ERR=0)
SPA=50000
PRA=A_OFFSET;BGA;MCA
ENDIF
MG "A: DONE!"{EA};A_BUSY=0;EN
#B_ALL
B_BUSY=1;B_SPD=2;JS #B_SPEED;IF (_TSB&$04=$00);PRB=500000;BGB;MCB;ENDIF
PRB=-20000000;BGB;MG "B: DRIVING NEGATIVE TO LIMIT SWITCH"{EA}
#B_LP01;IF(_TSB&$02=$02)|(_TI0&$04=$04);ACB=500000;SPB=125000;ENDIF
IF(_TI0&$08=$08);ACB=200000;SPB=50000;ENDIF;JP #B_LP01,(_TSB&$04=$04)
MCB;MG "B: NEGATIVE LIMIT SWITCH TRIPPED"{EA}
MG "B: DRIVING FORWARD SLOWLY WATCHING FOR LIMIT SWITCH TO CLEAR"{EA}
B_SPD=1;JS #B_SPEED;PRB=500000;BGB
#B_LP02;JP #B_LP02,(_TSB&$04=$00);STB;MCB
DPB=0;MG "B: LIMIT SWITCH CLEAR - ZERO DEFINED"{EA}
B_SPD=2;JS #B_SPEED;PRB=-1000000;BGB
MG "B: DRIVING BACK TO TRIP & STORING LOCATION OF LIMIT SWITCH"{EA}
#B_LP03;JP #B_LP03,(_TSB&$04=$04);MCB;B_LL_SET=_PAB
MG "B: LOW LIMIT SWITCH FOUND AT "{EA},B_LL_SET{EA} {F6.0}
B_SPD=2;JS #B_SPEED;PRB=2000000;BGB
MG "B: DRIVING FORWARD WATCHING FOR SW1 TO TRIP"{EA}
#B_LP04;JP #B_LP04,(_TI0&$08=$00);STB;MCB;B_S1_SET=_PAB
MG "B: SW1 TRIPPED AT "{EA},B_S1_SET{EA}{F6.0}
B_SPD=1;JS #B_SPEED;PRB=-500000
MG "B: DRIVING NEGATIVE SLOWLY WATCHING FOR SW1 TO CLEAR"{EA};BGB
#B_LP05;JP #B_LP05,(_TI0&$08=$08);STB;MCB;B_S1_CLR=_PAB
MG "B: SW1 CLEARED AT "{EA},B_S1_CLR{EA}{F6.0}
MG "B: DRIVING FORWARD WATCHING FOR SW2 TO TRIP"{EA}
PRB=10000000;B_SPD=3;JS #B_SPEED;BGB
#B_LP06;JP #B_LP06,(_TI0&$04=$00);STB;MCB
B_S2_SET=_PAB;MG "B: SW2 TRIPPED AT "{EA},B_S2_SET{EA}{F7.0}
B_SPD=1;JS #B_SPEED;PRB=-500000
MG "B: DRIVING NEGATIVE SLOWLY WATCHING FOR SW2 TO CLEAR"{EA};BGB
#B_LP07;JP #B_LP07,(_TI0&$04=$04);STB;MCB;B_S2_CLR=_PAB
MG "B: SW2 CLEARED AT "{EA},B_S2_CLR{EA}{F7.0}
MG "B: DRIVING FORWARD WATCHING FOR SW3 TO TRIP"{EA}
PRB=15000000;B_SPD=3;JS #B_SPEED;BGB
#B_LP08;JP #B_LP08,(_TSB&$02=$00);STB;MCB;B_S3_SET=_PAB
MG "B: SW3 TRIPPED AT "{EA},B_S3_SET{EA}{F8.0}
B_SPD=2;JS #B_SPEED;PRB=-500000
MG "B: DRIVING NEGATIVE WATCHING FOR SW3 TO CLEAR"{EA};BGB
#B_LP09;JP #B_LP09,(_TSB&$02=$02);STB;MCB;B_S3_CLR=_PAB
MG "B: SW3 CLEARED AT "{EA},B_S3_CLR{EA}{F8.0}
MG "B: DRIVING FORWARD WATCHING FOR HIGH LIMIT TO TRIP"{EA}
PRB=10000000;B_SPD=2;JS #B_SPEED;BGB
#B_LP10;JP #B_LP10,(_TSB&$08=$08);MCB;B_HL_SET=_PAB
MG "B: HIGH LIMIT SWITCH FOUND AT "{EA},B_HL_SET{EA}{F8.0}
B_SPD=2;JS #B_SPEED;PRB=-500000
MG "B: DRIVING NEGATIVE SLOWLY WATCHING FOR HIGH LIMIT TO CLEAR"{EA};BGB
#B_LP11;JP #B_LP11,(_TSB&$08=$00);MCB;B_HL_CLR=_PAB
MG "B: HIGH LIMIT SWITCH CLEARED AT "{EA},B_HL_CLR{EA}{F8.0}
MG ""{EA}
MG "B_LL_SET/B_LL_CLR"{EA},B_LL_SET{EA}{F5.0},"/0"{EA}
MG "B_S1_SET/B_S1_CLR"{EA},B_S1_SET{EA}{F6.0},"/"{EA},B_S1_CLR{EA}{F6.0}
MG "B_S2_SET/B_S2_CLR"{EA},B_S2_SET{EA}{F7.0},"/"{EA},B_S2_CLR{EA}{F7.0}
MG "B_S3_SET/B_S3_CLR"{EA},B_S3_SET{EA}{F8.0},"/"{EA},B_S3_CLR{EA}{F8.0}
MG "B_HL_SET/B_HL_CLR"{EA},B_HL_SET{EA}{F8.0},"/"{EA},B_HL_CLR{EA}{F8.0}
MG "B: DONE!"{EA};B_BUSY=0;EN
#B_ZERO;B_BUSY=1;B_POS=0;B_SPD=4;JS #B_SPEED
IF(_TSB&$04=$00);MG "B: LOW LIMIT ALREADY ACTUATED - DRIVING POSITIVE"{EA}
PRB=100000;BGB;MCB;ENDIF
PAB=B_LL_SET;MG "B: DRIVING TO B_LL_SET"{EA};BGB;MCB
IF(_TSB&$04=$04)
MG "B: LOW LIMIT SWITCH NOT FOUND WHERE EXPECTED - SEARCHING"{EA}
B_SPD=2;JS #B_SPEED;PRB=-20000000;BGB
#B_LP12;IF(_TSB&$02=$02)|(_TI0&$04=$04);ACB=500000;SPB=125000;ENDIF
IF(_TI0&$08=$08);ACB=200000;SPB=50000;ENDIF;JP #B_LP12,(_TSB&$04=$04)&(_BGB=1)
MCB;ENDIF
IF(_TSB&$04=$00)
MG "B: LOWER LIMIT TRIPPED, WATCHING FOR RESET"{EA};B_SPD=1;JS #B_SPEED
PRB=SEARCH;BGB
#B_LP13;JP #B_LP13,(_TSB&$04=$00)&(_BGB=1);STB;MCB
ELSE;MG "COULD NOT FIND NEGATIVE LIMIT SWITCH"{EA};B_POS=0;B_ERR=1;ENDIF
IF(_TSB&$04=$04);DPB=0;MG "B: ZERO DEFINED AT LOWER LIMIT"{EA};B_POS=0;B_ERR=0
ELSE;MG "B: LOWER LIMIT CLEAR NOT FOUND IN SPECIFIED SEARCH DISTBNCE"{EA}
ENDIF;MG "B: DONE!"{EA};B_BUSY=0;EN
#B_SW1;B_BUSY=1;B_POS=0;B_SPD=4;JS #B_SPEED;PAB=B_S1_CLR-SHTCLR
MG "B: DRIVING TO B_S1_CLR-SHTCLR"{EA};BGB;MCB
B_SPD=2;JS #B_SPEED;PAB=B_S1_SET-SHTSET;MG "B: DRIVING TO B_S1_SET-SHTSET"{EA}
BGB;MCB;B_SPD=1;JS #B_SPEED;PRB=SEARCH;MG "B: WATCHING FOR SW1 TO SET"{EA};BGB
#B_LP14
JP #B_LP14,(_TI0&$08=$00)&(_BGB=1);STB;MCB
IF(_TI0&$08=$08)&(_PAB>(B_S1_SET-SHTSET))
B_S1_CUR=_PAB;B_POS=1;B_ERR=0;MG "B: SW1 SET @"{EA},B_S1_CUR{EA} {F6.0}
MG "B: B_S1_SET="{EA},B_S1_SET{EA} {F6.0};ENDIF
IF(_TI0&$08=$08)&(_PAB=(B_S1_SET-SHTSET))
MG "B: SW1 FOUND BEFORE MOTOR MOVED"{EA};ENDIF
IF(_TI0&$08=$00)&(_PAB=(B_S1_SET-SHTSET+SEARCH))
B_POS=-1;B_ERR=1;MG "B: SW1 NOT FOUND WITHIN SPECIFIED DISTBNCE"{EA};ENDIF
IF(_TSB&04=$00)
B_POS=-1;B_ERR=1;MG "B: SW1 - NEGATIVE LIMIT TRIPPED"{EA};ENDIF
IF(_TSB&08=$00)
B_POS=-1;B_ERR=1;MG "B: SW1 - POSITIVE LIMIT TRIPPED"{EA};ENDIF
IF(B_ERR=0)
SPB=50000
PRB=B_OFFSET;BGB;MCB
ENDIF
MG "B: DONE!"{EA};B_BUSY=0;EN
#B_SW2;B_BUSY=1;B_SPD=4;JS #B_SPEED;PAB=B_S2_CLR-SHTCLR
MG "B: DRIVING TO B_S2_CLR - SHTCLR"{EA};BGB;MCB;B_SPD=2;JS #B_SPEED
PAB=B_S2_SET-SHTSET;MG "B: DRIVING TO B_S2_SET - SHTSET"{EA};BGB;MCB
B_SPD=1;JS #B_SPEED;PRB=SEARCH;MG "B: WATCHING FOR SW2 TO SET"{EA};BGB
#B_LP15
JP #B_LP15,(_TI0&$04=$00)&(_BGB=1);STB;MCB
IF(_TI0&$04=$04)&(_PAB>(B_S2_SET-SHTSET))
B_S2_CUR=_PAB;MG "B: SW2 SET @"{EA},B_S2_CUR{EA} {F7.0}
MG "B: B_S2_SET="{EA},B_S2_SET{EA} {F7.0};B_POS=2;B_ERR=0;ENDIF
IF(_TI0&$04=$04)&(_PAB=(B_S2_SET-SHTSET))
MG "B: SW2 FOUND BEFORE MOTOR MOVED"{EA};ENDIF
IF(_TI0&$04=$00)&(_PAB=(B_S2_SET-SHTSET+SEARCH))
B_POS=-1;B_ERR=1;MG "B: SW2 NOT FOUND WITHIN SPECIFIED DISTBNCE"{EA};ENDIF
IF(_TSB&04=$00)
B_POS=-1;B_ERR=1;MG "B: SW2 - NEGATIVE LIMIT TRIPPED"{EA};ENDIF
IF(_TSB&08=$00)
B_POS=-1;B_ERR=1;MG "B: SW2 - POSITIVE LIMIT TRIPPED"{EA};ENDIF
IF(B_ERR=0)
SPB=50000
PRB=B_OFFSET;BGB;MCB
ENDIF
MG "B: DONE!"{EA};B_BUSY=0;EN
#B_SW3;B_BUSY=1;B_SPD=4;JS #B_SPEED;PAB=B_S3_CLR-SHTCLR
MG "B: DRIVING TO B_S3_CLR - SHTCLR"{EA};BGB;MCB
B_SPD=2;JS #B_SPEED;PAB=B_S3_SET-SHTSET
MG "B: DRIVING TO B_S3_SET - SHTSET"{EA};BGB;MCB
B_SPD=1;JS #B_SPEED;PAB=B_S3_SET-SHTSET+SEARCH
MG "B: WATCHING FOR SW3 TO SET"{EA};BGB
#B_LP16
JP #B_LP16,(_TSB&$02=$00)&(_BGB=1);STB;MCB
IF(_TSB&$02=$02)&(_PAB>(B_S3_SET-SHTSET))
B_S3_CUR=_PAB;MG "B: SW3 SET @"{EA},B_S3_CUR{EA} {F8.0}
MG "B: B_S3_SET="{EA},B_S3_SET{EA} {F8.0};B_POS=3;B_ERR=0;ENDIF
IF(_TSB&$02=$02)&(_PAB=(B_S3_SET-SHTSET))
MG "B: SW3 FOUND BEFORE MOTOR MOVED"{EA};ENDIF
IF(_TSB&$02=$00)&(_PAB=(B_S3_SET-SHTSET+SEARCH))
B_POS=-1;B_ERR=1;MG "B: SW3 NOT FOUND WITHIN SPECIFIED DISTBNCE"{EA};ENDIF
IF(_TSB&04=$00)
B_POS=-1;B_ERR=1;MG "B: SW3 - NEGATIVE LIMIT TRIPPED"{EA};ENDIF
IF(_TSB&08=$00)
B_POS=-1;B_ERR=1;MG "B: SW3 - POSITIVE LIMIT TRIPPED"{EA};ENDIF
IF(B_ERR=0)
SPB=50000
PRB=B_OFFSET;BGB;MCB
ENDIF
MG "B: DONE!"{EA};B_BUSY=0;EN
#C_ALL
C_BUSY=1;C_SPD=2;JS #C_SPEED;IF (_TSC&$04=$00);PRC=500000;BGC;MCC;ENDIF
PRC=-20000000;BGC;MG "C: DRIVING NEGATIVE TO LIMIT SWITCH"{EA}
#C_LP01;IF(_TSC&$02=$02)|(_TI0&$10=$10);ACC=500000;SPC=125000;ENDIF
IF(_TI0&$20=$20);ACC=200000;SPC=50000;ENDIF;JP #C_LP01,(_TSC&$04=$04)
MCC;MG "C: NEGATIVE LIMIT SWITCH TRIPPED"{EA}
MG "C: DRIVING FORWARD SLOWLY WATCHING FOR LIMIT SWITCH TO CLEAR"{EA}
C_SPD=1;JS #C_SPEED;PRC=500000;BGC
#C_LP02;JP #C_LP02,(_TSC&$04=$00);STC;MCC;DPC=0
MG "C: LIMIT SWITCH CLEAR - ZERO DEFINED"{EA}
C_SPD=2;JS #C_SPEED;PRC=-1000000;BGC
MG "C: DRIVING BACK TO TRIP & STORING LOCATION OF LIMIT SWITCH"{EA}
#C_LP03;JP #C_LP03,(_TSC&$04=$04);MCC;C_LL_SET=_PAC
MG "C: LOW LIMIT SWITCH FOUND AT "{EA},C_LL_SET{EA} {F6.0}
C_SPD=2;JS #C_SPEED;PRC=2000000;BGC
MG "C: DRIVING FORWARD WATCHING FOR SW1 TO TRIP"{EA}
#C_LP04;JP #C_LP04,(_TI0&$20=$00);STC;MCC;C_S1_SET=_PAC
MG "C: SW1 TRIPPED AT "{EA},C_S1_SET{EA}{F6.0}
C_SPD=1;JS #C_SPEED;PRC=-500000
MG "C: DRIVING NEGATIVE SLOWLY WATCHING FOR SW1 TO CLEAR"{EA};BGC
#C_LP05;JP #C_LP05,(_TI0&$20=$20);STC;MCC;C_S1_CLR=_PAC
MG "C: SW1 CLEARED AT "{EA},C_S1_CLR{EA}{F6.0}
MG "C: DRIVING FORWARD WATCHING FOR SW2 TO TRIP"{EA}
PRC=10000000;C_SPD=3;JS #C_SPEED;BGC
#C_LP06;JP #C_LP06,(_TI0&$10=$00);STC;MCC;C_S2_SET=_PAC
MG "C: SW2 TRIPPED AT "{EA},C_S2_SET{EA}{F7.0}
C_SPD=1;JS #C_SPEED;PRC=-500000
MG "C: DRIVING NEGATIVE SLOWLY WATCHING FOR SW2 TO CLEAR"{EA};BGC
#C_LP07;JP #C_LP07,(_TI0&$10=$10);STC;MCC;C_S2_CLR=_PAC
MG "C: SW2 CLEARED AT "{EA},C_S2_CLR{EA}{F7.0}
MG "C: DRIVING FORWARD WATCHING FOR SW3 TO TRIP"{EA}
PRC=15000000;C_SPD=3;JS #C_SPEED;BGC
#C_LP08;JP #C_LP08,(_TSC&$02=$00);STC;MCC;C_S3_SET=_PAC
MG "C: SW3 TRIPPED AT "{EA},C_S3_SET{EA}{F8.0}
C_SPD=2;JS #C_SPEED;PRC=-500000
MG "C: DRIVING NEGATIVE WATCHING FOR SW3 TO CLEAR"{EA};BGC
#C_LP09;JP #C_LP09,(_TSC&$02=$02);STC;MCC;C_S3_CLR=_PAC
MG "C: SW3 CLEARED AT "{EA},C_S3_CLR{EA}{F8.0}
MG "C: DRIVING FORWARD WATCHING FOR HIGH LIMIT TO TRIP"{EA}
PRC=10000000;C_SPD=2;JS #C_SPEED;BGC
#C_LP10;JP #C_LP10,(_TSC&$08=$08);MCC;C_HL_SET=_PAC
MG "C: HIGH LIMIT SWITCH FOUND AT "{EA},C_HL_SET{EA}{F8.0}
C_SPD=2;JS #C_SPEED;PRC=-500000
MG "C: DRIVING NEGATIVE SLOWLY WATCHING FOR HIGH LIMIT TO CLEAR"{EA};BGC
#C_LP11;JP #C_LP11,(_TSC&$08=$00);MCC;C_HL_CLR=_PAC
MG "C: HIGH LIMIT SWITCH CLEARED AT "{EA},C_HL_CLR{EA}{F8.0}
MG ""{EA};MG "C_LL_SET/C_LL_CLR"{EA},C_LL_SET{EA}{F5.0},"/0"{EA}
MG "C_S1_SET/C_S1_CLR"{EA},C_S1_SET{EA}{F6.0},"/"{EA},C_S1_CLR{EA}{F6.0}
MG "C_S2_SET/C_S2_CLR"{EA},C_S2_SET{EA}{F7.0},"/"{EA},C_S2_CLR{EA}{F7.0}
MG "C_S3_SET/C_S3_CLR"{EA},C_S3_SET{EA}{F8.0},"/"{EA},C_S3_CLR{EA}{F8.0}
MG "C_HL_SET/C_HL_CLR"{EA},C_HL_SET{EA}{F8.0},"/"{EA},C_HL_CLR{EA}{F8.0}
MG "C: DONE!"{EA};C_BUSY=0;EN
#C_ZERO;C_BUSY=1;C_POS=0;C_SPD=4;JS #C_SPEED
IF(_TSC&$04=$00);MG "C: LOW LIMIT ALREADY ACTUATED - DRIVING POSITIVE"{EA}
PRC=100000;BGC;MCC;ENDIF
PAC=C_LL_SET;MG "C: DRIVING TO C_LL_SET"{EA};BGC;MCC
IF(_TSC&$04=$04)
MG "C: LOW LIMIT SWITCH NOT FOUND WHERE EXPECTED - SEARCHING"{EA}
C_SPD=2;JS #C_SPEED;PRC=-20000000;BGC
#C_LP12;IF(_TSC&$02=$02)|(_TI0&$10=$10);ACC=500000;SPC=125000;ENDIF
IF(_TI0&$20=$20);ACC=200000;SPC=50000;ENDIF;JP #C_LP12,(_TSC&$04=$04)&(_BGC=1)
MCC;ENDIF
IF(_TSC&$04=$00)
MG "C: LOWER LIMIT TRIPPED, WATCHING FOR RESET"{EA};C_SPD=1;JS #C_SPEED
PRC=SEARCH;BGC
#C_LP13
JP #C_LP13,(_TSC&$04=$00)&(_BGC=1);STC;MCC
ELSE
MG "COULD NOT FIND NEGATIVE LIMIT SWITCH"{EA};C_POS=0;C_ERR=1;ENDIF
IF(_TSC&$04=$04);DPC=0;MG "C: ZERO DEFINED AT LOWER LIMIT"{EA};C_POS=0;C_ERR=0
ELSE;MG "C: LOWER LIMIT CLEAR NOT FOUND IN SPECIFIED SEARCH DISTANCE"{EA}
ENDIF;MG "C: DONE!"{EA};C_BUSY=0;EN
#C_SW1;C_BUSY=1;C_POS=0
C_SPD=4;JS #C_SPEED;PAC=C_S1_CLR-SHTCLR;MG "C: DRIVING TO C_S1_CLR-SHTCLR"{EA}
BGC;MCC
C_SPD=2;JS #C_SPEED;PAC=C_S1_SET-SHTSET;MG "C: DRIVING TO C_S1_SET-SHTSET"{EA}
BGC;MCC
C_SPD=1;JS #C_SPEED;PRC=SEARCH;MG "C: WATCHING FOR SW1 TO SET"{EA};BGC
#C_LP14
JP #C_LP14,(_TI0&$20=$00)&(_BGC=1);STC;MCC
IF(_TI0&$20=$20)&(_PAC>(C_S1_SET-SHTSET))
C_S1_CUR=_PAC;C_POS=1;C_ERR=0;MG "C: SW1 SET @"{EA},C_S1_CUR{EA} {F6.0}
MG "C: C_S1_SET="{EA},C_S1_SET{EA} {F6.0};ENDIF
IF(_TI0&$20=$20)&(_PAC=(C_S1_SET-SHTSET))
MG "C: SW1 FOUND BEFORE MOTOR MOVED"{EA};ENDIF
IF(_TI0&$20=$00)&(_PAC=(C_S1_SET-SHTSET+SEARCH))
C_POS=-1;C_ERR=1;MG "C: SW1 NOT FOUND WITHIN SPECIFIED DISTANCE"{EA};ENDIF
IF(_TSC&04=$00)
C_POS=-1;C_ERR=1;MG "C: SW1 - NEGATIVE LIMIT TRIPPED"{EA};ENDIF
IF(_TSC&08=$00)
C_POS=-1;C_ERR=1;MG "C: SW1 - POSITIVE LIMIT TRIPPED"{EA};ENDIF
IF(C_ERR=0)
SPC=50000
PRC=C_OFFSET;BGC;MCC
ENDIF
MG "C: DONE!"{EA};C_BUSY=0;EN
#C_SW2;C_BUSY=1;C_SPD=4;JS #C_SPEED;PAC=C_S2_CLR-SHTCLR
MG "C: DRIVING TO C_S2_CLR - SHTCLR"{EA};BGC;MCC
C_SPD=2;JS #C_SPEED;PAC=C_S2_SET-SHTSET
MG "C: DRIVING TO C_S2_SET - SHTSET"{EA};BGC;MCC
C_SPD=1;JS #C_SPEED;PRC=SEARCH;MG "C: WATCHING FOR SW2 TO SET"{EA};BGC
#C_LP15
JP #C_LP15,(_TI0&$10=$00)&(_BGC=1);STC;MCC
IF(_TI0&$10=$10)&(_PAC>(C_S2_SET-SHTSET))
C_S2_CUR=_PAC;MG "C: SW2 SET @"{EA},C_S2_CUR{EA} {F7.0}
MG "C: C_S2_SET="{EA},C_S2_SET{EA} {F7.0};C_POS=2;C_ERR=0;ENDIF
IF(_TI0&$10=$10)&(_PAC=(C_S2_SET-SHTSET))
MG "C: SW2 FOUND BEFORE MOTOR MOVED"{EA};ENDIF
IF(_TI0&$10=$00)&(_PAC=(C_S2_SET-SHTSET+SEARCH))
C_POS=-1;C_ERR=1;MG "C: SW2 NOT FOUND WITHIN SPECIFIED DISTANCE"{EA};ENDIF
IF(_TSC&04=$00)
C_POS=-1;C_ERR=1;MG "C: SW2 - NEGATIVE LIMIT TRIPPED"{EA};ENDIF
IF(_TSC&08=$00)
C_POS=-1;C_ERR=1;MG "C: SW2 - POSITIVE LIMIT TRIPPED"{EA};ENDIF
IF(C_ERR=0)
SPC=50000
PRC=C_OFFSET;BGC;MCC
ENDIF
MG "C: DONE!"{EA};C_BUSY=0;EN
#C_SW3;C_BUSY=1;C_SPD=4;JS #C_SPEED;PAC=C_S3_CLR-SHTCLR
MG "C: DRIVING TO C_S3_CLR - SHTCLR"{EA};BGC;MCC
C_SPD=2;JS #C_SPEED;PAC=C_S3_SET-SHTSET
MG "C: DRIVING TO C_S3_SET - SHTSET"{EA};BGC;MCC
C_SPD=1;JS #C_SPEED;PAC=C_S3_SET-SHTSET+SEARCH
MG "C: WATCHING FOR SW3 TO SET"{EA};BGC
#C_LP16
JP #C_LP16,(_TSC&$02=$00)&(_BGC=1);STC;MCC
IF(_TSC&$02=$02)&(_PAC>(C_S3_SET-SHTSET))
C_S3_CUR=_PAC;MG "C: SW3 SET @"{EA},C_S3_CUR{EA} {F8.0}
MG "C: C_S3_SET="{EA},C_S3_SET{EA} {F8.0};C_POS=3;C_ERR=0;ENDIF
IF(_TSC&$02=$02)&(_PAC=(C_S3_SET-SHTSET))
MG "C: SW3 FOUND BEFORE MOTOR MOVED"{EA};ENDIF
IF(_TSC&$02=$00)&(_PAC=(C_S3_SET-SHTSET+SEARCH))
C_POS=-1;C_ERR=1;MG "C: SW3 NOT FOUND WITHIN SPECIFIED DISTANCE"{EA};ENDIF
IF(_TSC&04=$00)
C_POS=-1;C_ERR=1;MG "C: SW3 - NEGATIVE LIMIT TRIPPED"{EA};ENDIF
IF(_TSC&08=$00)
C_POS=-1;C_ERR=1;MG "C: SW3 - POSITIVE LIMIT TRIPPED"{EA};ENDIF
IF(C_ERR=0)
SPC=50000
PRC=C_OFFSET;BGC;MCC
ENDIF
MG "C: DONE!"{EA};C_BUSY=0;EN
#D_ALL
D_BUSY=1;D_SPD=2;JS #D_SPEED;IF (_TSD&$04=$00);PRD=500000;BGD;MCD;ENDIF
PRD=-20000000;BGD;MG "D: DRIVING NEGATIVE TO LIMIT SWITCH"{EA}
#D_LP01;IF(_TSD&$02=$02)|(_TI0&$40=$40);ACD=500000;SPD=125000;ENDIF
IF(_TI0&$80=$80);ACD=200000;SPD=50000;ENDIF;JP #D_LP01,(_TSD&$04=$04)
MCD;MG "D: NEGATIVE LIMIT SWITCH TRIPPED"{EA}
MG "D: DRIVING FORWARD SLOWLY WATCHING FOR LIMIT SWITCH TO CLEAR"{EA}
D_SPD=1;JS #D_SPEED;PRD=500000;BGD
#D_LP02;JP #D_LP02,(_TSD&$04=$00);STD;MCD;DPD=0
MG "D: LIMIT SWITCH CLEAR - ZERO DEFINED"{EA}
D_SPD=2;JS #D_SPEED;PRD=-1000000;BGD
MG "D: DRIVING BACK TO TRIP & STORING LOCATION OF LIMIT SWITCH"{EA}
#D_LP03;JP #D_LP03,(_TSD&$04=$04);MCD;D_LL_SET=_PAD
MG "D: LOW LIMIT SWITCH FOUND AT "{EA},D_LL_SET{EA} {F6.0}
D_SPD=2;JS #D_SPEED;PRD=2000000;BGD
MG "D: DRIVING FORWARD WATCHING FOR SW1 TO TRIP"{EA}
#D_LP04;JP #D_LP04,(_TI0&$80=$00);STD;MCD;D_S1_SET=_PAD
MG "D: SW1 TRIPPED AT "{EA},D_S1_SET{EA}{F6.0}
D_SPD=1;JS #D_SPEED;PRD=-500000
MG "D: DRIVING NEGATIVE SLOWLY WATCHING FOR SW1 TO CLEAR"{EA};BGD
#D_LP05;JP #D_LP05,(_TI0&$80=$80);STD;MCD;D_S1_CLR=_PAD
MG "D: SW1 CLEARED AT "{EA},D_S1_CLR{EA}{F6.0}
MG "D: DRIVING FORWARD WATCHING FOR SW2 TO TRIP"{EA}
PRD=10000000;D_SPD=3;JS #D_SPEED;BGD
#D_LP06;JP #D_LP06,(_TI0&$40=$00);STD;MCD;D_S2_SET=_PAD
MG "D: SW2 TRIPPED AT "{EA},D_S2_SET{EA}{F7.0}
D_SPD=1;JS #D_SPEED;PRD=-500000
MG "D: DRIVING NEGATIVE SLOWLY WATCHING FOR SW2 TO CLEAR"{EA};BGD
#D_LP07;JP #D_LP07,(_TI0&$40=$40);STD;MCD;D_S2_CLR=_PAD
MG "D: SW2 CLEARED AT "{EA},D_S2_CLR{EA}{F7.0}
MG "D: DRIVING FORWARD WATCHING FOR SW3 TO TRIP"{EA}
PRD=15000000;D_SPD=3;JS #D_SPEED;BGD
#D_LP08;JP #D_LP08,(_TSD&$02=$00);STD;MCD;D_S3_SET=_PAD
MG "D: SW3 TRIPPED AT "{EA},D_S3_SET{EA}{F8.0}
D_SPD=2;JS #D_SPEED;PRD=-500000
MG "D: DRIVING NEGATIVE WATCHING FOR SW3 TO CLEAR"{EA};BGD
#D_LP09;JP #D_LP09,(_TSD&$02=$02);STD;MCD;D_S3_CLR=_PAD
MG "D: SW3 CLEARED AT "{EA},D_S3_CLR{EA}{F8.0}
MG "D: DRIVING FORWARD WATCHING FOR HIGH LIMIT TO TRIP"{EA}
PRD=10000000;D_SPD=2;JS #D_SPEED;BGD
#D_LP10;JP #D_LP10,(_TSD&$08=$08);MCD;D_HL_SET=_PAD
MG "D: HIGH LIMIT SWITCH FOUND AT "{EA},D_HL_SET{EA}{F8.0}
D_SPD=2;JS #D_SPEED;PRD=-500000
MG "D: DRIVING NEGATIVE SLOWLY WATCHING FOR HIGH LIMIT TO CLEAR"{EA};BGD
#D_LP11;JP #D_LP11,(_TSD&$08=$00);MCD;D_HL_CLR=_PAD
MG "D: HIGH LIMIT SWITCH CLEARED AT "{EA},D_HL_CLR{EA}{F8.0}
MG ""{EA}
MG "D_LL_SET/D_LL_CLR"{EA},D_LL_SET{EA}{F5.0},"/0"{EA}
MG "D_S1_SET/D_S1_CLR"{EA},D_S1_SET{EA}{F6.0},"/"{EA},D_S1_CLR{EA}{F6.0}
MG "D_S2_SET/D_S2_CLR"{EA},D_S2_SET{EA}{F7.0},"/"{EA},D_S2_CLR{EA}{F7.0}
MG "D_S3_SET/D_S3_CLR"{EA},D_S3_SET{EA}{F8.0},"/"{EA},D_S3_CLR{EA}{F8.0}
MG "D_HL_SET/D_HL_CLR"{EA},D_HL_SET{EA}{F8.0},"/"{EA},D_HL_CLR{EA}{F8.0}
MG "D: DONE!"{EA};D_BUSY=0;EN
#D_ZERO;D_BUSY=1;D_POS=0;D_SPD=4;JS #D_SPEED
IF(_TSD&$04=$00);MG "D: LOW LIMIT ALREADY ACTUATED - DRIVING POSITIVE"{EA}
PRD=100000;BGD;MCD;ENDIF
PAD=D_LL_SET;MG "D: DRIVING TO D_LL_SET"{EA};BGD;MCD
IF(_TSD&$04=$04)
MG "D: LOW LIMIT SWITCH NOT FOUND WHERE EXPECTED - SEARCHING"{EA}
D_SPD=2;JS #D_SPEED;PRD=-20000000;BGD
#D_LP12;IF(_TSD&$02=$02)|(_TI0&$40=$40);ACD=500000;SPD=125000;ENDIF
IF(_TI0&$80=$80);ACD=200000;SPD=50000;ENDIF;JP #D_LP12,(_TSD&$04=$04)&(_BGD=1)
MCD;ENDIF
IF(_TSD&$04=$00)
MG "D: LOWER LIMIT TRIPPED, WATCHING FOR RESET"{EA};D_SPD=1;JS #D_SPEED
PRD=SEARCH;BGD
#D_LP13
JP #D_LP13,(_TSD&$04=$00)&(_BGD=1);STD;MCD
ELSE
MG "COULD NOT FIND NEGATIVE LIMIT SWITCH"{EA};D_POS=0;D_ERR=1;ENDIF
IF(_TSD&$04=$04);DPD=0;MG "D: ZERO DEFINED AT LOWER LIMIT"{EA};D_POS=0
D_ERR=0;ELSE
MG "D: LOWER LIMIT CLEAR NOT FOUND IN SPECIFIED SEARCH DISTDNCE"{EA};ENDIF
MG "D: DONE!"{EA};D_BUSY=0;EN
#D_SW1;D_BUSY=1;D_POS=0
D_SPD=4;JS #D_SPEED;PAD=D_S1_CLR-SHTCLR;MG "D: DRIVING TO D_S1_CLR-SHTCLR"{EA}
BGD;MCD
D_SPD=2;JS #D_SPEED;PAD=D_S1_SET-SHTSET;MG "D: DRIVING TO D_S1_SET-SHTSET"{EA}
BGD;MCD
D_SPD=1;JS #D_SPEED;PRD=SEARCH;MG "D: WATCHING FOR SW1 TO SET"{EA};BGD
#D_LP14
JP #D_LP14,(_TI0&$80=$00)&(_BGD=1);STD;MCD
IF(_TI0&$80=$80)&(_PAD>(D_S1_SET-SHTSET))
D_S1_CUR=_PAD;D_POS=1;D_ERR=0;MG "D: SW1 SET @"{EA},D_S1_CUR{EA} {F6.0}
MG "D: D_S1_SET="{EA},D_S1_SET{EA} {F6.0};ENDIF
IF(_TI0&$80=$80)&(_PAD=(D_S1_SET-SHTSET))
MG "D: SW1 FOUND BEFORE MOTOR MOVED"{EA};ENDIF
IF(_TI0&$80=$00)&(_PAD=(D_S1_SET-SHTSET+SEARCH))
D_POS=-1;D_ERR=1;MG "D: SW1 NOT FOUND WITHIN SPECIFIED DISTDNCE"{EA};ENDIF
IF(_TSD&04=$00)
D_POS=-1;D_ERR=1;MG "D: SW1 - NEGATIVE LIMIT TRIPPED"{EA};ENDIF
IF(_TSD&08=$00)
D_POS=-1;D_ERR=1;MG "D: SW1 - POSITIVE LIMIT TRIPPED"{EA};ENDIF
IF(D_ERR=0)
SPD=50000
PRD=D_OFFSET;BGD;MCD
ENDIF
MG "D: DONE!"{EA};D_BUSY=0;EN
#D_SW2;D_BUSY=1;D_SPD=4;JS #D_SPEED;PAD=D_S2_CLR-SHTCLR
MG "D: DRIVING TO D_S2_CLR - SHTCLR"{EA};BGD;MCD
D_SPD=2;JS #D_SPEED;PAD=D_S2_SET-SHTSET
MG "D: DRIVING TO D_S2_SET - SHTSET"{EA};BGD;MCD
D_SPD=1;JS #D_SPEED;PRD=SEARCH;MG "D: WATCHING FOR SW2 TO SET"{EA};BGD
#D_LP15
JP #D_LP15,(_TI0&$40=$00)&(_BGD=1);STD;MCD
IF(_TI0&$40=$40)&(_PAD>(D_S2_SET-SHTSET))
D_S2_CUR=_PAD;MG "D: SW2 SET @"{EA},D_S2_CUR{EA} {F7.0}
MG "D: D_S2_SET="{EA},D_S2_SET{EA} {F7.0};D_POS=2;D_ERR=0;ENDIF
IF(_TI0&$40=$40)&(_PAD=(D_S2_SET-SHTSET))
MG "D: SW2 FOUND BEFORE MOTOR MOVED"{EA};ENDIF
IF(_TI0&$40=$00)&(_PAD=(D_S2_SET-SHTSET+SEARCH))
D_POS=-1;D_ERR=1;MG "D: SW2 NOT FOUND WITHIN SPECIFIED DISTDNCE"{EA};ENDIF
IF(_TSD&04=$00)
D_POS=-1;D_ERR=1;MG "D: SW2 - NEGATIVE LIMIT TRIPPED"{EA};ENDIF
IF(_TSD&08=$00)
D_POS=-1;D_ERR=1;MG "D: SW2 - POSITIVE LIMIT TRIPPED"{EA};ENDIF
IF(D_ERR=0)
SPD=50000
PRD=D_OFFSET;BGD;MCD
ENDIF
MG "D: DONE!"{EA};D_BUSY=0;EN
#D_SW3;D_BUSY=1;D_SPD=4;JS #D_SPEED;PAD=D_S3_CLR-SHTCLR
MG "D: DRIVING TO D_S3_CLR - SHTCLR"{EA};BGD;MCD
D_SPD=2;JS #D_SPEED;PAD=D_S3_SET-SHTSET
MG "D: DRIVING TO D_S3_SET - SHTSET"{EA};BGD;MCD
D_SPD=1;JS #D_SPEED;PAD=D_S3_SET-SHTSET+SEARCH
MG "D: WATCHING FOR SW3 TO SET"{EA};BGD
#D_LP16
JP #D_LP16,(_TSD&$02=$00)&(_BGD=1);STD;MCD
IF(_TSD&$02=$02)&(_PAD>(D_S3_SET-SHTSET))
D_S3_CUR=_PAD;MG "D: SW3 SET @"{EA},D_S3_CUR{EA} {F8.0}
MG "D: D_S3_SET="{EA},D_S3_SET{EA} {F8.0};D_POS=3;D_ERR=0;ENDIF
IF(_TSD&$02=$02)&(_PAD=(D_S3_SET-SHTSET))
MG "D: SW3 FOUND BEFORE MOTOR MOVED"{EA};ENDIF
IF(_TSD&$02=$00)&(_PAD=(D_S3_SET-SHTSET+SEARCH))
D_POS=-1;D_ERR=1;MG "D: SW3 NOT FOUND WITHIN SPECIFIED DISTDNCE"{EA};ENDIF
IF(_TSD&04=$00)
D_POS=-1;D_ERR=1;MG "D: SW3 - NEGATIVE LIMIT TRIPPED"{EA};ENDIF
IF(_TSD&08=$00)
D_POS=-1;D_ERR=1;MG "D: SW3 - POSITIVE LIMIT TRIPPED"{EA};ENDIF
IF(D_ERR=0)
SPD=50000
PRD=D_OFFSET;BGD;MCD
ENDIF
MG "D: DONE!"{EA};D_BUSY=0;EN
